/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package anglesensor;

import dynamixel.ComPortFindParam;
import dynamixel.ComPortFinder;
import java.util.Arrays;
import java.util.logging.Level;
import java.util.logging.Logger;
import jssc.SerialPort;
import jssc.SerialPortEvent;
import jssc.SerialPortEventListener;
import jssc.SerialPortException;
import org.math.plot.utils.Array;

/**
 *
 * @author minaev
 */
public class CameraSensor {
    public static final int HORZ_AXIS = 2;
    public static final int VERT_AXIS = 3;
    public static final int LINE_AXIS = 7;
    public static final int BUT1_AXIS = 4;
    public static final int BUT2_AXIS = 5;
    
    
    ComPortFindParam cpfp = new ComPortFindParam(38400, 8, 1, 0, 60, 3000) {
        @Override
        public boolean isFinded(int[] pckg) {
            boolean res = true;
            char syms[] = {'X', 'Y', 'O', 'Z', 'T', 'K', '=', 0x9, 0x0d, 0x0a};
            boolean insyms;
            boolean isContain[] = new boolean[syms.length];

            for (int i = 0; i < pckg.length; i++) {
                insyms = false;
                for (int j = 0; j < syms.length; j++) {
                    if (syms[j] == pckg[i]) {
                        isContain[j] = true;
                        insyms = true;
                    }
                }
                if (!(insyms || pckg[i] == '-'
                        || (pckg[i] >= '0' && pckg[i] <= '9'))) {
                    res = false;
                }
            }

            if (!res) {
                return false;
            }

            for (int i = 0; i < isContain.length; i++) {
                res &= isContain[i];
            }

            return res;
        }

        @Override
        public int[] getSendPckg() {
            try {
                Thread.sleep(3000);
            } catch (InterruptedException ex) {
                Logger.getLogger(CameraSensor.class.getName()).log(Level.SEVERE, null, ex);
            }
            return null;
        }
    };
    final int cntParam = 8;
    int params[] = new int[cntParam];
    String[] paramsString = {"X=", "Y=", "K=", "O=", "K1=", "K2=", "Z=", "T="};
    SerialPort sp;
    SerialPortEventListener spel = new SerialPortEventListener() {
        @Override
        public void serialEvent(SerialPortEvent spe) {
            try {
                int[] res = sp.readIntArray();
                for (int i = 0; i < res.length; i++) {
                    for (int j = 0; j < cntParam; j++) {
                        //System.out.println("param test "+j+": "+(char)res[i]+"=="+paramsString[j].charAt(0));
                        if (res[i] == paramsString[j].charAt(0)) {
                            //System.out.println("res[i] == charAt(0): "+(char)res[i]);
                            if (i + paramsString[j].length() < res.length) {
                                //System.out.println("lenth right");
                                boolean goal = true;
                                for (int k = 0; k < paramsString[j].length(); k++) {
                                    if (res[i + k] != paramsString[j].charAt(k)) {
                                        //System.out.println("GUF char is: "+(char)res[i+k]+" "+paramsString[j].charAt(k));
                                        goal = false;
                                    }
                                }
                                if (goal) {
                                    i += paramsString[j].length();
                                    String digstr = "";
                                    while ((i + 1 < res.length)
                                            && ((res[i] >= '0' && res[i] <= '9')
                                            || res[i] == '-')) {
                                        digstr += (char) res[i];
                                        i++;
                                    }
                                    if (res[i] == 0xd || res[i] == 0xa
                                            || res[i] == 0x9) {
                                        params[j] = Integer.valueOf(digstr);
                                    }
                                }
                            }
                        }
                    }
                }
            } catch (SerialPortException ex) {
                Logger.getLogger(CameraSensor.class.getName()).log(Level.SEVERE, null, ex);
            }

        }
    };

    public CameraSensor() throws SerialPortException {
        String serialName = ComPortFinder.findCom(cpfp);
        if (serialName != null) {
            sp = new SerialPort(serialName);
            sp.openPort();
            sp.setParams(38400, 8, 1, 0);
            sp.addEventListener(spel);
        } else {
            System.out.println("CameraSersor COM is null");
        }
    }
    
    public String strVals(){
        String res = "";
        for (int i = 0; i < cntParam; i++) {
            res+=paramsString[i]+params[i]+ " ";
        }
        return res;
    }

    public static void main(String[] args) throws SerialPortException, InterruptedException {
        CameraSensor cs = new CameraSensor();
        
        while(true){
            System.out.println(cs.strVals());
            Thread.sleep(100);
        }

    }
    
    public int getParam(int indx){
        if(indx<cntParam){
            return params[indx];
        }else{
            return -1;
        }
            
    }
}
